#include "ch.h"
#include "hal.h"
#include "hw_conf.h"
#include "stm32f4xx_conf.h"

void gpio_init(void)
{
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
    
    palSetPadMode(EN_GATE_GPIO, EN_GATE_PIN, PAL_MODE_OUTPUT_PUSHPULL |
                        PAL_STM32_OSPEED_HIGHEST);
    palClearPad(EN_GATE_GPIO, EN_GATE_PIN);

    // PWM
    palSetPadMode(PWM_A_HIGH_GPIO, PWM_A_HIGH_PIN, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
            PAL_STM32_OSPEED_HIGHEST |
            PAL_STM32_PUPDR_FLOATING);
    palSetPadMode(PWM_B_HIGH_GPIO, PWM_B_HIGH_PIN, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
            PAL_STM32_OSPEED_HIGHEST |
            PAL_STM32_PUPDR_FLOATING);
    palSetPadMode(PWM_C_HIGH_GPIO, PWM_C_HIGH_PIN, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
            PAL_STM32_OSPEED_HIGHEST |
            PAL_STM32_PUPDR_FLOATING);
    palSetPadMode(PWM_A_LOW_GPIO, PWM_A_LOW_PIN, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
            PAL_STM32_OSPEED_HIGHEST |
            PAL_STM32_PUPDR_FLOATING);
    palSetPadMode(PWM_B_LOW_GPIO, PWM_B_LOW_PIN, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
            PAL_STM32_OSPEED_HIGHEST |
            PAL_STM32_PUPDR_FLOATING);
    palSetPadMode(PWM_C_LOW_GPIO, PWM_C_LOW_PIN, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
            PAL_STM32_OSPEED_HIGHEST |
            PAL_STM32_PUPDR_FLOATING);

    // ADC
    palSetPadMode(CURR_A_GPIO, CURR_A_PIN, PAL_MODE_INPUT_ANALOG);
    palSetPadMode(CURR_B_GPIO, CURR_B_PIN, PAL_MODE_INPUT_ANALOG);
    palSetPadMode(CURR_C_GPIO, CURR_C_PIN, PAL_MODE_INPUT_ANALOG);
    palSetPadMode(VSENSE_A_GPIO, VSENSE_A_PIN, PAL_MODE_INPUT_ANALOG);
    palSetPadMode(VSENSE_B_GPIO, VSENSE_B_PIN, PAL_MODE_INPUT_ANALOG);
    palSetPadMode(VSENSE_C_GPIO, VSENSE_C_PIN, PAL_MODE_INPUT_ANALOG);
    palSetPadMode(VBUS_GPIO, VBUS_PIN, PAL_MODE_INPUT_ANALOG);
    palSetPadMode(NTC_GPIO, NTC_PIN, PAL_MODE_INPUT_ANALOG);

#if defined(INFINITY_V4_0) || defined(INFINITY_V3_0)
    // WS2812B
    palSetPadMode(WS2812B_GPIO, WS2812B_PIN,
            PAL_MODE_ALTERNATE(GPIO_AF_TIM3) |
            PAL_STM32_OTYPE_PUSHPULL |
            PAL_STM32_OSPEED_MID1);
#else
    // RGB LED
    palSetPadMode(LED_R_GPIO, LED_R_PIN, PAL_MODE_ALTERNATE(GPIO_AF_TIM8) |
            PAL_STM32_OTYPE_PUSHPULL |
            PAL_STM32_OSPEED_MID1);
    palSetPadMode(LED_G_GPIO, LED_G_PIN, PAL_MODE_ALTERNATE(GPIO_AF_TIM8) |
            PAL_STM32_OTYPE_PUSHPULL |
            PAL_STM32_OSPEED_MID1);
    palSetPadMode(LED_B_GPIO, LED_B_PIN, PAL_MODE_ALTERNATE(GPIO_AF_TIM8) |
            PAL_STM32_OTYPE_PUSHPULL |
            PAL_STM32_OSPEED_MID1);
#endif

    // USB
    palSetPadMode(GPIOA, 11, PAL_MODE_ALTERNATE(10));
    palSetPadMode(GPIOA, 12, PAL_MODE_ALTERNATE(10));

    // CAN
    palSetPadMode(CAN_RX_GPIO, CAN_RX_PIN, PAL_MODE_ALTERNATE(GPIO_AF_CAN1) |
            PAL_STM32_OTYPE_PUSHPULL |
            PAL_STM32_OSPEED_MID1);
    palSetPadMode(CAN_TX_GPIO, CAN_TX_PIN, PAL_MODE_ALTERNATE(GPIO_AF_CAN1) |
            PAL_STM32_OTYPE_PUSHPULL |
            PAL_STM32_OSPEED_MID1);
}
